

#ifndef _MOTOR_CTRL_DC_H
#define _MOTOR_CTRL_DC_H

#include "stdint.h"
#include "arm_math.h"
#include "pid_controller.h"

#define  CTRL_MODE_DEBUG     1
#define  CTRL_MODE_RELEASE   2

#define  SYS_CTRL_MODE       CTRL_MODE_RELEASE

#define  FSM_MOTOR_IDLE  0
#define  FSM_MOTOR_RUN   1
#define  FSM_MOTOR_BREAK 2
#define  FSM_MOTOR_FAULT 3
#define  FSM_MOTOR_STOP  2


#define  NUM_MOTOR_BRUSH_MAIN  0
#define  NUM_MOTOR_BRUSH_LEFT  1
#define  NUM_MOTOR_BRUSH_RIGHT 2
#define  NUM_MOTOR_DUST_FAN    3

#define  NUM_MOTOR_WHEEL_LEFT  10
#define  NUM_MOTOR_WHEEL_RIGHT 11

#define  NUM_MOTOR_MOP_LEFT    12
#define  NUM_MOTOR_MOP_RIGHT   13

#define  MOTOR_IDLE_PWM_CAMP  (uint16_t)(0.064 * PWM_PERIOD_REG)

#define  MAIN_BRUSH_PWM_DUTY  1000
#define  EDGE_BRUSH_PWM_DUTY  2000
#define  DUST_FAN_PWM_DUTY    1500

#if 0
#define  WHEEL_DIAMETER_M     0.0598f
#else
#define  WHEEL_DIAMETER_M     0.072f  // meidi wheel parameter
#endif

#define  LINEAR_SPEED_SF      (WHEEL_DIAMETER_M * 3.1415726f)
#define  INV_LINEAR_SPEED_SF  (1.0f / LINEAR_SPEED_SF)
#define  ANGLE_SPEED_SF       (180.0f / 3.1415726f)

#define  BREAK_DISTANCE_SF    1000.0f * (LINEAR_SPEED_SF / ENCODER_LINE_NUM)
#define  MAGNET_BREAK_DISTANCE_SF  1000.0f * (LINEAR_SPEED_SF / MAGNET_ENCODER_LINE_NUM)

#if 0
#define  WHEEL_DISTANCE_M          0.224f
#else
#define  WHEEL_DISTANCE_M          0.25f
#endif

#define  HALF_WHEEL_DISTANCE_M     (WHEEL_DISTANCE_M * 0.5f)

#define  WHEEL_DIFFENCE_SPEED_SF   (HALF_WHEEL_DISTANCE_M * INV_LINEAR_SPEED_SF)

#define  WHEEL_SPEED_MAX      1.8f
#define  ANGLE_SPEED_MAX      200.0f

#define  CLIFF_BACK_SPEED     -1.0f
#define  STOP_WAIT_TIME       50

typedef struct
{
	uint16_t  motor_num;
	uint16_t  FSM;
	float32_t vdc_now;
	float32_t speedref_rps;
	float32_t speedfb_rps;
	
	ENUM_PID_MODE  pid_mode;
	uint16_t  stop_cnt;
	
	uint16_t  fault;

}STR_MOTOR_CTRL;


#define MOP_UP_RUN_TIME   400
#define MOP_DOWN_RUN_TIME 200


typedef enum
{
	MOP_FSM_UNKNOWN = 0,
	MOP_FSM_STOP,
	MOP_FSM_RUN_UP,
	MOP_FSM_RUN_DOWN,
	MOP_FSM_FAULT,

}ENUM_MOP;


typedef struct
{
	ENUM_MOP FSM;
	
	uint8_t motor_num;	
	
	uint8_t enable;
	uint8_t up_pos_flg;
	uint8_t exist_flg;
	
	uint16_t holdtime_cnt;

	uint16_t up_run_duty;
	uint16_t down_run_duty;
	
}STR_MOP_CTRL;
















void motorctrl_dc(void);
void motorctrl_init(void);
void motorctrl_wheel(void);


#endif
